#include <algorithm>
#include <cmath>

#include "Vector3.h"
#include "Matrix33.h"
#include "Quaternion.h"

using namespace gti;

void matrix33::identity()
{
	m[0]=1; m[3]=0; m[6]=0;
	m[1]=0; m[4]=1; m[7]=0;
	m[2]=0; m[5]=0; m[8]=1;
}

void matrix33::reset()
{
	memset(m, 0, 9*sizeof(float));
}

void matrix33::transpose()
{
   std::swap(m[1],m[3]); std::swap(m[2],m[6]); std::swap(m[5],m[7]);
}

void matrix33::multTranspose(const vector3f& v, vector3f& ov) const
{
	matrix33 T = *this; T.transpose();
	ov = T * v;
/*
	ov.x = m[0] * v.x + m[1] * v.y + m[2] * v.z; 
	ov.y = m[3] * v.x + m[4] * v.y + m[5] * v.z; 
	ov.z = m[6] * v.x + m[7] * v.y + m[8] * v.z;
*/
}

void matrix33::getQuaternion(quaternion& q) const
{
	float trace = _11 + _22 + _33;

    if ( trace > 0 )
    {
        // |w| > 1/2, may as well choose w > 1/2
        float fRoot = sqrt(trace + 1.0f);  // 2w
        q.w = 0.5f*fRoot;
        fRoot = 0.5f/fRoot;  // 1/(4w)
        q.x = (_32-_23)*fRoot;
        q.y = (_13-_31)*fRoot;
        q.z = (_21-_12)*fRoot;
    }
    else
    {
        // |w| <= 1/2
        int i = 0;
        if ( _22 > _11 )
            i = 1;
        if ( _33 > M[i][i] )
            i = 2;
        int j = (i+1) % 3;
        int k = (j+1) % 3;

        float fRoot = sqrtf( M[i][i]- M[j][j]- M[k][k]+1.0f);
        float* apfQuat[3] =  { &q[0], &q[1], &q[2] };  
        *apfQuat[i] = 0.5f*fRoot;
        fRoot = 0.5f/fRoot;
        q.w = (M[k][j]-M[j][k])*fRoot;
        *apfQuat[j] = (M[j][i]+M[i][j])*fRoot;
        *apfQuat[k] = (M[k][i]+M[i][k])*fRoot;
    }
}

float matrix33::trace() const
{
    return _11+_22+_33;
}

void matrix33::set(const float *src)
{
	memcpy(m, src, 9*sizeof(float));
}

matrix33 matrix33::operator+(const matrix33 &matrix) const
{
	matrix33 res;
	for (int i=0;i<9;i++) // TODO: unroll
	  res[i] = m[i]+matrix[i];
	return res;
}

matrix33 matrix33::operator-(const matrix33 &matrix) const
{
	matrix33 res;
	for (int i=0;i<9;i++) // TODO: unroll
	  res[i] = m[i]-matrix[i];
	return res;
}

matrix33 matrix33::operator*(const matrix33& matrix) const
{
	matrix33 ret;
	unsigned int i,j,k;
	for (i=0;i<3;i++) 	
	{
		for (j=0;j<3;j++) 
		{
			ret.M[i][j]=0.0;
			for (k=0;k<3;k++) 
				ret.M[i][j] += M[i][k] * matrix.M[k][j];
		}
	}
	return ret;
}

matrix33 gti::operator*(const matrix33& matrix, float f)
{
	matrix33 res;
	for (int i=0;i<9;i++) // TODO: unroll
	  res[i] = matrix[i]*f;
	return res;
}

matrix33 gti::operator*(float f,const matrix33& matrix)
{
	matrix33 res;
	for (int i=0;i<9;i++) // TODO: unroll
	  res[i] = matrix[i]*f;
	return res;
}

matrix33 matrix33::operator*=(const matrix33& matrix)
{
  matrix33 res = *this*matrix;
  *(this) = res;
  return *this; 
}

matrix33 matrix33::operator*=(float f)
{
	matrix33 res;
	for (int i=0;i<9;i++) // TODO: unroll loop
	  res[i] = m[i]*f;
  *(this) = res;
  return *this; 
}

vector3f gti::operator*(const matrix33& matrix, const vector3f& v)
{
   vector3f vector;
   vector.x = matrix.m[0] * v.x + matrix.m[3] * v.y + matrix.m[6] * v.z; 
   vector.y = matrix.m[1] * v.x + matrix.m[4] * v.y + matrix.m[7] * v.z; 
   vector.z = matrix.m[2] * v.x + matrix.m[5] * v.y + matrix.m[8] * v.z;
   return vector;
}


bool gti::operator == (const matrix33& m1, const matrix33& m2)
{
	return (!memcmp(m1.m,m2.m,9));
}

bool gti::operator != (const matrix33& m1, const matrix33& m2)
{
	return !(m1 == m2);
}


matrix33 operator+(const matrix33& matrix,float f)
{
	matrix33 res;
	for (int i=0;i<9;i++) // TODO: unroll
	  res[i] = matrix.m[i]+f;
	return res;
}

matrix33 operator+(float f,const matrix33& matrix)
{
	matrix33 res;
	for (int i=0;i<9;i++) // TODO: unroll
	  res[i] = matrix.m[i]+f;
	return res;
}

matrix33 operator-(const matrix33& matrix,float f)
{
	matrix33 res;
	for (int i=0;i<9;i++) // TODO: unroll
	  res[i] = matrix.m[i]-f;
	return res;
}

matrix33 operator-(float f,const matrix33& matrix)
{
	matrix33 res;
	for (int i=0;i<9;i++) // TODO: unroll
	  res[i] = matrix.m[i]-f;
	return res;
}

void matrix33::setRow(int row, const vector3f& axis)
{
	m[row] = axis.x;
	m[row+4] = axis.y;
	m[row+8] = axis.z;
}

void matrix33::setColumn(int col, const vector3f& axis)
{
	const int idx = col*3;
	m[idx] = axis.x;
	m[idx+1] = axis.y;
	m[idx+2] = axis.z;
}

void matrix33::copyRow(int id, vector3f& row) const
{
	row.x = m[id];
	row.y = m[id+3];
	row.z = m[id+6];
}

void matrix33::copyColumn(int id, vector3f& column) const
{
	const int idx = id*3;
	column.x = m[idx];
	column.y = m[idx+1];
	column.z = m[idx+2];
}

void matrix33::setScaleMatrix(const vector3f& scale)
{
	_11 = scale.x;	_12 = 0;		_13 = 0;
	_21 = 0;		_22 = scale.y;	_23 = 0;
	_31 = 0;		_32 = 0;		_33 = scale.z;
}

bool matrix33::inverse(matrix33& inverse) const
{
   unsigned int i, j, k, swap;
   float t;
   matrix33 temp;
   inverse.identity();

   temp = (*this);

   unsigned int m,n;
   m = n = 3;
	
   for (i = 0; i < m; i++)
   {
      // Look for largest element in column

      swap = i;
      for (j = i + 1; j < m; j++)// m or n
	  {
//         if ( fabs(temp(j,i)) > fabs(temp(i,i)) )
		 if ( fabs(temp(j,i)) > fabs(temp(swap,i)) )
            swap = j;
	  }
   
      if (swap != i)
      {
         // Swap rows.

         for (k = 0; k < n; k++)
         {
			 std::swap(temp(i,k),temp(swap,k));
			 std::swap(inverse(i,k),inverse(swap,k));
         }
      }

      // No non-zero pivot.  The CMatrix is singular, which shouldn't
      // happen.  This means the user gave us a bad CMatrix.


#define MATRIX_SINGULAR_THRESHOLD 0.0001

      if ( fabsf(temp(i,i)) <= MATRIX_SINGULAR_THRESHOLD)
	  {
		  inverse.identity();
         return false;
	  }
#undef MATRIX_SINGULAR_THRESHOLD

      t = 1.0f/temp(i,i);

      for (k = 0; k < n; k++)//m or n
      {
         temp(i,k) *= t;
         inverse(i,k) *= t;
      }

      for (j = 0; j < m; j++) // m or n
      {
         if (j != i)
         {
            t = temp(j,i);
            for (k = 0; k < n; k++)//m or n
            {
               temp(j,k) -= (temp(i,k) * t);
               inverse(j,k) -= (inverse(i,k) * t);
            }
         }
      }
   }

   return true;
}

void matrix33::orthonormalize()
{
	// to do: test matrix33::orthonormalize
	/*
    vector3f X(_11,_21,_31);
    vector3f Y(_12,_22,_32);
    vector3f Z;

    X.normalize();
    Z = CrossProduct(X,Y);
	Z.normalize();
    Y = CrossProduct(Z,X);
	Y.normalize();

    _11 = X(0); _12 = Y(0); _13 = Z(0);
    _21 = X(1); _22 = Y(1); _23 = Z(1);
    _31 = X(2); _32 = Y(2); _33 = Z(2);
	*/
	
	vector3f x(m[0],m[1],m[2]);
	vector3f y(m[3],m[4],m[5]);
	vector3f z;
	x.normalize();
	z = CrossProduct(x,y);
	z.normalize();
	y = CrossProduct(z,x);
	y.normalize();
	m[0] = x.x; m[3] = y.x; m[6] = z.x;
	m[1] = x.y; m[4] = y.y; m[7] = z.y;
	m[2] = x.z; m[5] = y.z; m[8] = z.z;
}


void matrix33::setLookAt( vector3f zaxis ) {
	vector3f xaxis,yaxis;
	zaxis.normalize();
	vector3f up( 0.0, 1.0f, 0.0f );
	float dotp = DotProduct(up,zaxis);
	// angle between them is 180 
	if (fabsf(dotp) > 0.999999f) up.set(1.0f, 0.0f, 0.0f);
	xaxis = CrossProduct(up, zaxis); 
	xaxis.normalize();
	yaxis = CrossProduct(zaxis, xaxis);
	
	_11 = xaxis.x; _12 = yaxis.x; _13 = zaxis.x; 
	_21 = xaxis.y; _22 = yaxis.y; _23 = zaxis.y; 
	_31 = xaxis.z; _32 = yaxis.z; _33 = zaxis.z; 
}
